import cv2
import numpy as np
import math
import json
# from pwm import CarMove
num_lane_point = 4

# 载入小车控制参数
filepath="car.json"
file=open(filepath,"r")
car_para=json.load(file)

def initial_process(img):
    gray_img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)  # rgb to gray
    img = cv2.blur(gray_img, (5, 5))  # 均值滤波
    img=img[100:][:]    #删除图像中的远景背景
    retval, dst = cv2.threshold(img, 0, 255, cv2.THRESH_OTSU)  # 二值化
    shape=dst.shape #获取图像尺寸
    dst_above=dst[:shape[0]//2][:]
    dst_under=dst[shape[0]//2:][:]
    # dst_above = cv2.dilate(dst_above, None, iterations=3)  # 上半区膨胀，白区域变大
    # dst_under = cv2.dilate(dst_under, None, iterations=5)  # 下半区膨胀，白区域变大
    dst=np.concatenate((dst_above,dst_under),axis=0)
    # dst = cv2.erode(dst, None, iterations=4)  # 腐蚀，白区域变小
    # cv2.imshow("img",dst)
    # cv2.waitKey(10)
    return dst

def cal_k(dst,base):
    height, width = dst.shape
    half_width = int(width / 2)
    right_line_pos = np.zeros((num_lane_point, 1))
    left_line_pos = np.zeros((num_lane_point, 1))
    k_l=0
    k_r=0
    for i in range(num_lane_point):
        detect_height = base - 15* (i + 1)
        detect_area_left = dst[detect_height,0: half_width - 1]
        detect_area_right = dst[detect_height, half_width: width - 1]
        line_left = np.where(detect_area_left == 0)
        line_right = np.where(detect_area_right == 0)
        # print(line_left,'and',line_right)
        if len(line_left[0]):
            left_line_pos[i] = int(np.max(line_left))#存储着采样行的左端点位置
        else:
            left_line_pos[i] = 0
        if len(line_right[0]):
            right_line_pos[i] = int(np.min(line_right))#存储着采样行的右端点位置
        else:
            right_line_pos[i] = 0
        # print(left_line_pos[i],right_line_pos[i])
    left_line_pos_dec = np.nonzero(left_line_pos)  # 非零值索引,求斜率用
    right_line_pos_dec = np.nonzero(right_line_pos)  # 非零值索引，求斜率用
    #print(len(right_line_pos_dec[0]))
    #左右斜率，2+点才可有斜率
    if len(left_line_pos_dec[0])>= 2:
        maxleftline=max(left_line_pos_dec[0])
        minleftline=min(left_line_pos_dec[0])
        k_l=(left_line_pos[maxleftline][0]-left_line_pos[minleftline][0])/(maxleftline-minleftline)
        if k_l==0:k_l=1
    if len(right_line_pos_dec[0]) >=2:
        maxrightline=max(right_line_pos_dec[0])
        minrightline=min(right_line_pos_dec[0])
        k_r=(right_line_pos[maxrightline][0]-right_line_pos[minrightline][0])/(maxrightline-minrightline)
        if k_r==0:k_r=1
    if (k_r-k_l<10 or k_r-k_l>-10 )and (k_l>60 or k_l<-60):
        if k_l>0:k_r=0
        elif k_r<0:k_l=0
    return [k_l,k_r]

def decision_k(dst,base,historyangle,cnt):
    s="%.1f"%cnt+" "
    if historyangle>=30 or historyangle<=-30:
        historyangle=0
    angle=historyangle
    [k_l,k_r]=cal_k(dst,base)
    # print(k_l,k_r,base)
    if k_r and k_l:  # 如果左右都有斜率
        s+="左右都有斜率"
        if 0<k_l and k_l<20 and -20<k_r and k_r<0:#直走
            angle=0
        elif k_l < 0 and k_r < -20:  # 同向左转
            angle = -30
        elif k_r > 0 and k_l > 20:
            angle=30
        else:
            if base>car_para["left_base_check"]:#这里可以变大一些，就，加上上半部分噪声/其他物品的像素，现在取50
                angle=decision_k(dst,base-20,historyangle,cnt+0.1)#异向，往往处在拐角，进一步预判，调试，是否拐早了
            else:
                angle=0
    elif k_l == 0 and k_r:   # 如果检测不到左侧黑线
        s+="左侧无黑线"
        if k_r<-25:  # 左转
            if base>=car_para["speed_control_base_check3"]:
                angle=car_para['left_angle4']
            elif base>=car_para["speed_control_base_check2"]:
                angle=car_para['left_angle3']
            elif base>=car_para["speed_control_base_check1"]:
                angle=car_para['left_angle2']
            else:
                angle=car_para["left_angle1"]
        elif 45>k_r and k_r>20:
            if base>car_para["speed_control_base_check2"]:
                angle=40
            else:
                angle=20   #马上越界了赶紧跑
        elif k_r>=45:
            angle=70   #马上越界了赶紧跑
        else:
            if base>car_para['left_base_check'] :#这里可以变大一些，就，加上上半部分噪声/其他物品的像素，现在取50
                angle=decision_k(dst, base - 20,historyangle,cnt+0.1)  # 可能歪了，没采样到近处的线，往远看
            else:
                angle+=car_para["left_change"]

    elif k_r == 0 and k_l:  # 如果检测不到右侧黑线
        angle=-20
        s+="右侧无黑线"
        if k_l > 25:   # 右转
            if base>=car_para["speed_control_base_check3"]:
                angle=car_para['right_angle4']
            elif base>=car_para["speed_control_base_check2"]:
                angle=car_para['right_angle3']
            elif base>=car_para["speed_control_base_check1"]:
                angle=car_para['right_angle2']
            else:
                angle=car_para['right_angle1']
        elif -45<k_l and k_l<=0:
            if base>car_para["speed_control_base_check2"]:
                angle=-40   #马上越界了赶紧跑
            else:
                angle=-20
        elif k_l<=-45:
            angle=-70   #马上越界了赶紧跑
        else:
            if base>car_para['right_base_check']:#这里可以变大一些，就，加上上半部分噪声/其他物品的像素，现在取50
                angle=decision_k(dst, base - 20,historyangle,cnt+0.1)  # 可能歪了，没采样到近处的线，往远看
            else:
                angle+=car_para["right_change"]

    else:  # 左右两侧都检测不到黑线，停止，挪线，向下挪，直到左/右有3+点，则右/左转
        s+="左右都无黑线"
        if base>car_para['back_check']:#这里可以变大一些，就，加上上半部分噪声/其他物品的像素，现在取50
            angle=decision_k(dst, base - 20,historyangle,cnt+0.1)  # 可能歪了，没采样到近处的线，往远看
        else:
            angle=-180
    # 还没有：转弯标志1+上3个动作有转弯，延续转弯，搜索，直到有线：向线走
    s+=" k_l="+str(k_l)+" k_r="+str(k_r)+" angle="+str(angle)+" base="+str(base)
    print(s)
    return angle

if __name__ == '__main__':
    # img = cv2.imread('E:/Pycharmfile/smartcar/imgs/img4.jpg')
    img = cv2.imread('C:/Users/blessing/Desktop/img/home/pi/Desktop/smart-car/img/60.jpg')
    # img = cv2.imread('C:/Users/blessing/Desktop/smart-car/img/img3.jpg')
    dst = initial_process(img)
    height, width = dst.shape
    base=int(2*height / 3)
    historyangle=0
    theta= decision_k(dst,base,historyangle)
    # print(theta)
    cv2.namedWindow('image', cv2.WINDOW_NORMAL)
    cv2.line(dst, (0,50), (640, 50), (0, 255, 0),2)
    cv2.line(dst, (0,100), (640, 100), (0, 255, 0),2)
    cv2.line(dst, (0,150), (640, 150), (0, 255, 0),2)
    cv2.line(dst, (0,200), (640, 200), (0, 255, 0),2)
    cv2.imshow('image', dst)
    cv2.waitKey(0)
    cv2.destroyAllWindows()
